﻿using System.Linq;
using System.Diagnostics;

namespace FanucTest
{
    using System;
    using System.Runtime.InteropServices;
    using System.Threading;
    using FanucLib;

    internal class Program
    {
        private static ConsoleKeyInfo KeyInfo;

        private static void Main(string[] args)
        {
            Trace.Listeners.Add(new ConsoleTraceListener() { Writer = Console.Out }); // 将控制台输出流添加到调试监视器列表中,调试输出同时输出到控制台

            Console.WriteLine("Press any key to start and press 'Q' to quit");
            Console.ReadKey();

            new Thread(() =>
            {
                Fanuc fanuc = new Fanuc("192.168.0.100"); // 初始化 指定机器人IP地址
                Stopwatch stopwatch = new Stopwatch();
                Trace.TraceInformation("Run");
                float value = 0;
                while (true)
                {
                    if (value > 100) value = 0;
                    value += 1;
                    Trace.TraceInformation("---Restart---");
                    stopwatch.Restart();
                    try
                    {
                        fanuc.WriteNumReg(10, value); // 向 数值型寄存器10 写入指定的值
                        var b = fanuc.ReadNumReg(10); // 读取 数值型寄存器10 的值
                        Trace.TraceInformation("数值型寄存器的值  = " + b.ToString());

                        fanuc.WritePosReg(10, value, 2.2f, 3.3f, 4.4f, 5.5f, 6.6f); // 将坐标写入 位置型寄存器10
                        var posValue = fanuc.ReadPosReg(10); // 读取位置型寄存器10 的值
                        Trace.TraceInformation("位置型寄存器的值 = " + string.Join(",", posValue.Select(_ => _.ToString()).ToArray()));

                        var curPos = fanuc.ReadCurPos(1); // 读取 指定用户坐标的机器人当前位置
                        Trace.TraceInformation("指定坐标系的位置 = " + string.Join(",", curPos.Select(_ => _.ToString()).ToArray()));
                    }
                    catch (Exception ex)
                    {
                        Trace.TraceError(ex.Message);
                        Trace.TraceError(ex.StackTrace);
                    }
                    Trace.TraceInformation($"---End {stopwatch.GetElapsedMilliseconds()} ms---");
                }
            })
            { IsBackground = true }.Start();

            while (char.ToUpper(Console.ReadKey().KeyChar) != 'Q') { }; // 按 q 键退出
            Trace.TraceInformation("Quit");
        }
    }

    /// <summary>
    /// Stopwatch Extension Class
    /// </summary>
    public static class StopwatchEx
    {
        /// <summary>
        /// Retrieves the frequency of the performance counter.
        /// </summary>
        /// <param name="lpFrequency">Hz</param>
        /// <returns>
        /// <para>If the installed hardware supports a high-resolution performance counter, the return value is nonzero.</para>
        /// <para>If the function fails, the return value is zero.</para>
        /// </returns>
        [DllImport("Kernel32.dll")]
        private static extern bool QueryPerformanceFrequency(out long lpFrequency);

        /// <summary>
        /// Get high-resolution elapsed milliseconds
        /// </summary>
        /// <returns>milliseconds</returns>
        /// <exception cref="NotSupportedException"/>
        public static double GetElapsedMilliseconds(this Stopwatch stopwatch)
        {
            long freq = 1;
            if (!QueryPerformanceFrequency(out freq))
                throw new NotSupportedException();
            return (double)stopwatch.ElapsedTicks * 1000 / freq;
        }
    }
}